#ifndef DIFFBOT_HW_INTERFACE_H
#define DIFFBOT_HW_INTERFACE_H


// ROS
#include <ros/ros.h>
#include <urdf/model.h>
#include <diffbot_msgs/EncodersStamped.h>
#include <diffbot_msgs/WheelsCmdStamped.h>
#include <diffbot_msgs/AngularVelocitiesStamped.h>
#include <sensor_msgs/JointState.h>

// ROS Controls
#include <hardware_interface/robot_hw.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/joint_command_interface.h>

#include <diffbot_base/pid.h>


namespace diffbot_base
{
    const unsigned int NUM_JOINTS = 2;

    struct JointState
    {
        float angular_position_;
        float angular_velocity_;
    };

    /// \brief 机器人的硬件接口
    class DiffBotHWInterface : public hardware_interface::RobotHW
    {
    public:
        /**
         * \brief 构造函数
         * \param nh - 话题的节点句柄
         * \param urdf_model - 可选的已解析机器人模型指针
         */
        DiffBotHWInterface(ros::NodeHandle &nh, urdf::Model *urdf_model = NULL);

        /** \brief 析构函数 */
        virtual ~DiffBotHWInterface() {}

        /** \brief init函数从非实时线程调用以初始化RobotHW
         *
         * 初始化自定义机器人是通过将关节句柄注册
         * (\ref hardware_interface::ResourceManager::registerHandle)到硬件
         * 接口,这些接口对类似的关节进行分组,并将这些单独的
         * 硬件接口注册到代表自定义机器人的类
         * (派生自此 \ref hardware_interface::RobotHW)
         *
         * \note 关节句柄和接口的注册可以在构造函数或此 \ref init 方法中完成
         *
         * \param root_nh 调用者命名空间根目录中的NodeHandle
         *
         * \param robot_hw_nh RobotHW应从其读取配置的命名空间中的NodeHandle
         *
         * \returns 如果初始化成功则返回True
         */
        virtual bool init(ros::NodeHandle &root_nh, ros::NodeHandle &robot_hw_nh);

        /** \brief 从机器人硬件读取数据
         *
         * read方法是控制循环周期的一部分(\ref read, update, \ref write)
         * 用于从机器人的硬件资源(关节、传感器、执行器)填充机器人状态。
         * 此方法应在controller_manager::ControllerManager::update()和\ref write之前调用。
         * 
         * \note \ref read名称指的是从硬件读取状态。
         * 这与\ref write相辅相成,后者指的是向硬件写入命令。
         *
         * 在\ref read中查询WallTime不是实时安全的。参数
         * \p time和\p period使得可以从实时源注入时间。
         *
         * \param time 当前时间
         * \param period 自上次调用\ref read以来经过的时间
         */
        virtual void read(const ros::Time& time, const ros::Duration& period) override;

        /** \brief 向机器人硬件写入命令
         * 
         * write方法是控制循环周期的一部分(\ref read, update, \ref write)
         * 用于向机器人的硬件资源(关节、执行器)发送命令。
         * 此方法应在\ref read和controller_manager::ControllerManager::update之后调用。
         * 
         * \note \ref write名称指的是向硬件写入命令。
         * 这与\ref read相辅相成,后者指的是从硬件读取状态。
         *
         * 在\ref write中查询WallTime不是实时安全的。参数
         * \p time和\p period使得可以从实时源注入时间。
         *
         * \param time 当前时间
         * \param period 自上次调用\ref write以来经过的时间
         */
        virtual void write(const ros::Time& time, const ros::Duration& period);

        /** \brief 检查是否收到测量的关节状态(位置和速度)
         * 
         * 此函数阻塞直到sub_measured_joint_states_订阅者在"measured_joint_states"话题上
         * 接收到每个关节的测量关节状态(角位置(rad)和角速度(rad/s)),
         * 类型为sensor_msgs::JointState。
         *
         * \param timeout 等待接收编码器计数的最小时间
         */
        bool isReceivingMeasuredJointStates(const ros::Duration &timeout=ros::Duration(1));

        /** \brief 用于调试关节状态的辅助函数 */
        virtual void printState();
        std::string printStateHelper();

        /** \brief 用于调试关节命令的辅助函数 */
        std::string printCommandHelper();

    protected:

        /** \brief 从参数服务器获取URDF XML */
        virtual void loadURDF(const ros::NodeHandle& nh, std::string param_name);

        /** \brief 接收来自Teensy MCU的编码器计数的回调函数 */
        void encoderTicksCallback(const diffbot_msgs::EncodersStamped::ConstPtr& msg_encoders);

        /** \brief 接收来自Teensy MCU的测量关节状态的回调函数 */
        void measuredJointStatesCallback(const sensor_msgs::JointState::ConstPtr& msg_joint_states);

        /** \brief 将编码器计数转换为弧度角度 */
        double ticksToAngle(const int &ticks) const;

        /** \brief 将角度标准化到[0, 360)范围内 */
        double normalizeAngle(double &angle) const;


        // 以下函数当前未使用
        // DiffBot直接从编码器计数计算标准化角度
        // ros_control的joint_commands被映射为电机驱动器的百分比值
        // 以下注释不正确
        /** \brief DiffBot报告以米为单位的行驶距离,ros_control RobotHW需要弧度 */
        double linearToAngular(const double &distance) const;
        /** \brief RobotHW提供rad/s的速度命令,DiffBot需要m/s */
        double angularToLinear(const double &angle) const;
        

        // 此类的简称
        std::string name_;

        // roscpp程序内部节点的启动和关闭
        ros::NodeHandle nh_;

        // 硬件接口
        // hardware_interface::JointStateInterface提供对所有关节值的读取访问
        // 而不会与其他控制器冲突
        hardware_interface::JointStateInterface joint_state_interface_;
        // hardware_interface::VelocityJointInterface继承自
        // hardware_interface::JointCommandInterface,用于读写
        // 关节速度。因为此接口保留关节的写访问权,
        // 可能会与写入相同关节的其他控制器发生冲突。
        // 要只读取关节速度,使用hardware_interface::JointStateInterface
        // 避免冲突。
        hardware_interface::VelocityJointInterface velocity_joint_interface_;

        // 配置
        std::vector<std::string> joint_names_;
        std::size_t num_joints_;
        urdf::Model *urdf_model_;

        double wheel_radius_;
        double wheel_diameter_;
        double max_velocity_;

        // 用于保存参数服务器中值的硬件相关参数
        // 参数在diffbot_base/config/base.yaml中定义
        double encoder_resolution_;
        // 增益修正模型的参数
        double gain_;
        double trim_;
        // 假设两个电机具有相同的电机常数
        double motor_constant_;
        double pwm_limit_;

        // 启用/禁用调试输出
        // 只能在硬件接口首次启动时设置
        // 以避免持续从参数服务器读取
        bool debug_;


        // 数据成员数组,用于存储发送到机器人资源(关节、执行器)的控制器命令
        // diff_drive_controller使用hardware_interface::VelocityJointInterface
        // 它为轮子关节提供语义含义,描述它们需要速度命令
        double joint_velocity_commands_[NUM_JOINTS];
 
        // 数据成员数组,用于存储机器人资源(关节、传感器)的状态
        // 这些值在read()方法中填充,并注册到
        // hardware_interface::JointStateInterface类型的joint_state_interface_
        double joint_positions_[NUM_JOINTS];
        double joint_velocities_[NUM_JOINTS];
        double joint_efforts_[NUM_JOINTS];

        ros::ServiceServer srv_start_;
        ros::ServiceServer srv_stop_;

        // 声明电机驱动器的发布者
        ros::Publisher pub_left_motor_value_;
        ros::Publisher pub_right_motor_value_;


        // 声明轮子关节角速度的发布者
        ros::Publisher pub_wheel_cmd_velocities_;

        // 声明重置轮子编码器的发布者
        // 在硬件接口首次启动时使用,以避免与之前运行的编码器计数有较大差异
        ros::Publisher pub_reset_encoders_;
        // 声明轮子编码器的订阅者
        // 此订阅者接收自定义diffbot_msgs/Encoder消息中的编码器计数
        ros::Subscriber sub_encoder_ticks_;
        // 声明测量关节状态的订阅者(轮子的绝对角位置(rad)和角速度(rad/s))
        // 此订阅者接收自定义diffbot_msgs/AngularVelocitiesStamped消息中的测量轮子关节角速度
        ros::Subscriber sub_measured_joint_states_;

        // 数组用于存储从\ref sub_encoder_ticks_订阅者接收的编码器计数值
        int encoder_ticks_[NUM_JOINTS];
        JointState measured_joint_states_[NUM_JOINTS];

        PID pids_[NUM_JOINTS];
    };  // class DiffBotHWInterface

}  // namespace

#endif // DIFFBOT_HW_INTERFACE_H
